# from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
import rclpy
from nav2_simple_commander.robot_navigator import BasicNavigator

def main():
    rclpy.init()
    navigator = BasicNavigator()
    # 必须先等待导航接口可用,再设置目标点
    navigator.waitUntilNav2Active() # Wait for Nav2 to be ready

    # 设置目标点
    goal_poses = []

    go_pose = PoseStamped()
    go_pose.header.frame_id = "map"
    go_pose.header.stamp = navigator.get_clock().now().to_msg()
    go_pose.pose.position.x = 1.0
    go_pose.pose.position.y = 1.0
    go_pose.pose.position.z = 0.0
    go_pose.pose.orientation.x = 0.0
    go_pose.pose.orientation.y = 0.0
    go_pose.pose.orientation.z = 0.0
    go_pose.pose.orientation.w = 1.0
    goal_poses.append(go_pose)

    go_pose1 = PoseStamped()
    go_pose1.header.frame_id = "map"
    go_pose1.header.stamp = navigator.get_clock().now().to_msg()
    go_pose1.pose.position.x = 2.0
    go_pose1.pose.position.y = 2.0
    go_pose1.pose.position.z = 0.0
    go_pose1.pose.orientation.x = 0.0
    go_pose1.pose.orientation.y = 0.0
    go_pose1.pose.orientation.z = 0.0
    go_pose1.pose.orientation.w = 1.0
    goal_poses.append(go_pose1)

    go_pose2 = PoseStamped()
    go_pose2.header.frame_id = "map"
    go_pose2.header.stamp = navigator.get_clock().now().to_msg()
    go_pose2.pose.position.x = 0.0
    go_pose2.pose.position.y = 0.0
    go_pose2.pose.position.z = 0.0
    go_pose2.pose.orientation.x = 0.0
    go_pose2.pose.orientation.y = 0.0
    go_pose2.pose.orientation.z = 0.0
    go_pose2.pose.orientation.w = 1.0
    goal_poses.append(go_pose2)

    # 开始导航
    navigator.followWaypoints(goal_poses)
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f"Feedback: 目标点编号: {feedback.current_waypoint}")
        # 超时取消函数 navigator.cancelTask()
    result = navigator.getResult()
    navigator.get_logger().info(f"Result: {result}")


    # rclpy.spin(navigator)
    # rclpy.shutdown()